/*
 *  component.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 15/11/07.
 *
 */

#include "component.h"

Component::Component()
:	dim(3),
	numDofs(0),
	numDofsInertia(0),
	pointNames(),
	mPoints(),
	localGlob(),
	localGlobInertia(),
	time(0.0),
	deltaT(0.0),
	matName(),
	q(),
	dq(),
	qC(),
	dqC(),
	Fi(),
	Fq(),
	Ki(),
	Kq(),
	Kdq(),
	M(),
	perturbation(1.0e-7),
	integratorType(0),
	thetab(),
	dofT(),
	globalTNum(),
	vLagMul(),
	augLagFlag(0),
	augConvergence(0),
	Phi(),
	penalty(1.0e10),
	FqLag(),
	FqPenalty()
{}

// ***

Component::Component(vector<string> vp, int di)
:	dim(di),
	numDofs(0),
	numDofsInertia(0),
	pointNames(vp),
	mPoints(),
	localGlob(),
	localGlobInertia(),
	time(0.0),
	deltaT(0.0),
	matName(),
	q(),
	dq(),
	qC(),
	dqC(),
	Fi(),
	Ki(),
	Kq(),
	Kdq(),
	M(),
	perturbation(1.0e-7),
	integratorType(0),
	thetab(),
	dofT(),
	globalTNum(),
	vLagMul(),
	augLagFlag(0),
	augConvergence(0),
	Phi(),
	penalty(1.0e10),
	FqLag(),
	FqPenalty()
{}
// ***

Component::Component(vector<string> vp, string mn, int di)
:	dim(di),
	numDofs(0),
	numDofsInertia(0),
	pointNames(vp),
	mPoints(),
	localGlob(),
	localGlobInertia(),
	time(0.0),
	deltaT(0.0),
	matName(mn),
	q(),
	dq(),
	Fi(),
	Ki(),
	Kdq(),
	M(),
	perturbation(1.0e-7),
	integratorType(0),
	thetab(),
	dofT(),
	globalTNum(),
	vLagMul(),
	augLagFlag(0),
	augConvergence(0),
	Phi(),
	penalty(1.0e10),
	FqLag(),
	FqPenalty()
{}

// ***

vector<string>& Component::getPointNames()
{
	return(pointNames);
}

// ***

void Component::setMPoints(map<string,Point*,less<string> > mp)
{
	mPoints=mp;
}

// ***

void Component::reportContent()
{
	map<string,Point*,less<string> >::iterator i;
	for(i=mPoints.begin(); i!=mPoints.end(); i++)	{
		cout << "								" << ((*i).second)->name << " at process\n" ;
	}
}

// ***

void Component::fillResidual(vector<double>& res_in, 
                                                        vector<double>& q_in, 
                                                        vector<double>& dq_in, 
                                                        const double& t, 
                                                        const int& modeFlag, 
                                                        const double& grav,
                                                        const int& gravDir,
                                                        const double& DeltaT)
{
	int i;
	getLocalDofs(q_in, dq_in);
	
	// modeFlag=0.  If possible, residual and tangent are calculated simultaneously. This is the case for the brick element with an implicit method
	// modeFlag=1.  Just residual is computed. This is the case of an explicit calculation, or an implicit calculation with numerical tangent
	calcFq(modeFlag);
	deltaT = DeltaT;
	time = t;
	// Add the Lagrange multiplier term

	if (augLagFlag==true)
	{
//		if (lTwoNorm(Fq)!=0.0)
		if (Fq.norm()>=1.0e-6)
		{
			FqRatio = lTwoNorm(FqPenalty)/Fq.norm();
		}
		else 
		{
//			if (lTwoNorm(FqPenalty)!=0.0) FqRatio = 1.0e12;	else FqRatio=0.0;
			FqRatio = 0.0;
		}
		//for(i=0; i<numDofs; i++)	Fq[i]+= FqLag[i];
	}
	
	calcFt(t,grav,gravDir);

	if(Fq.size()!=0)
	{
		for(i=0; i< numDofs; i++)	res_in[ localGlob[i] ]+= Fq [i];
	}
	if(Ft.size()!=0)
	{
		for(i=0; i< numDofs; i++)	res_in[ localGlob[i] ]-= Ft [i];
	}
}

// ***

void Component::fillResidual(	vector<double>& residue,
								const vector<double>& q,
								const vector<double>& qdot,
								const vector<double>& qddot,
								const double& t,
								const int& modeFlag,
								const double& grav, const int& gravDir,
//JC								const int& iType, const vector<double>& qConv, const vector<double>& dqConv,
 								const int& iType,
								const double& DeltaT
							)
{	
	int i;
	double coef;
//	integratorType = iType;
	deltaT = DeltaT;
	
	time = t;
	
	getLocalDofs(q, qdot);
/*
	cout << " qdot is " << endl;
	for(i=0; i<qdot.size(); ++i)	cout << " " << qdot[i];
	cout << endl;
*/
	
	calcFi(qddot);
	calcFq(modeFlag);

	
	//cout << "ltn(Fq) ****** = " << lTwoNorm(Fq) << endl;
	//cout << "ltn(FqPenal) = " << lTwoNorm(FqPenalty) << endl;

	// Add the Lagrange multiplier term

	if (augLagFlag==true)
	{
		/*
		cout << "Para q = " << endl;
		for(i=0; i<q.size(); ++i)	cout << q[i] << endl;
		
		cout << "Fqnorm is " << lTwoNorm(Fq) << endl;
		cout << "FqLagnorm is " << lTwoNorm(FqLag) << endl;
		*/
		if (Fq.norm()>=1.0e-6)
		{
			FqRatio = lTwoNorm(FqPenalty)/Fq.norm();
		}
		else 
		{
//			if (lTwoNorm(FqPenalty)!=0.0) FqRatio = 1.0e12;	else FqRatio=0.0;
			FqRatio = 0.0;
		}		

		//for(i=0; i<numDofs; i++)	Fq[i]+= FqLag[i];
	}
/*
	cout << "qdot is " << endl;
	for(i=0; i<qdot.size(); ++i)	cout << " " << qdot[i];
	cout << endl;
	
	cout << "dqC is "<< endl;
	printSTLVector(dqC);
 
	cout << "Fi is " << endl;
	printSTLVector(Fi);
*/
//	cout << "q es = " << endl;
//	for(i=0; i<q.size(); ++i)	cout << " " << q[i];
//	cout << "Fi es = " << endl;
//	printSTLVector(Fi);
	
	/*
	if (Fq.size()!=0) 
	{
		cout << "Fq is " << endl;
		cout << Fq << endl;
	}
	*/
/*
	cout << " q is " << endl;
	for(i=0; i<q.size(); ++i)	cout << " " << q[i];
	cout << endl;

	if (Fq.size()!=0) 
	{
		cout << "Fq is " << endl;
		//printSTLVector(Fq);
		cout << Fq << endl;
	}
*/
	/*
	if (Ft.size()!=0) 
	{
		cout << "Ft is " << endl;
		printSTLVector(Ft);
	}
	*/
	
	if (integratorType==1)	calcFt(t-DeltaT/2.0,grav,gravDir);	// Midpoint
	else	calcFt(t,grav,gravDir);
/*
	cout << "t = " << t << endl;
	cout << "DeltaT = " << DeltaT << endl;	
	cout << "Estoy calculando fuerzas en " << t-DeltaT << endl;
*/
	if(integratorType==0) coef = 1.0; 
	else coef = DeltaT;
	
	if(Fi.size()!=0)
	{
		for(i=0; i< numDofs; i++)	residue[ localGlob[i] ]+= Fi[i];
	}
	if(Fq.size()!=0)
	{
		for(i=0; i< numDofs; i++)	residue[ localGlob[i] ]+= coef*Fq[i];			
	}
	if(Ft.size()!=0)
	{
		for(i=0; i< numDofs; i++)	residue[ localGlob[i] ]-= coef*Ft[i];
	}
}

// ***

void Component::fillResFi(	vector<double>& residue,
					 const vector<double>& q,
					 const vector<double>& qddot,
					 const int& modeFlag
					 )
{
	int i;
	getLocalDofs(q);	
	calcFi(qddot);
	if(Fi.size()!=0)
	{
		for(i=0; i< numDofs; i++)	residue[ localGlob[i] ] += Fi [i];
	}	
}

// ***

void Component::fillResFq(	vector<double>& residue,
					 const vector<double>& q,
					 const vector<double>& qdot,
					 const int& modeFlag,
//JC					 const int& iType, const vector<double>& qConv, const vector<double>& dqConv,
  					 const int& iType,
					 const double& DeltaT
					 )
{
	int i;
	
//JC	getLocalDofs(q,qdot, qConv, dqConv);
	getLocalDofs(q,qdot);
	calcFq(modeFlag);
	
	double coef;
	if(integratorType==0) coef = 1.0; 
	else coef = DeltaT;

	if(Fq.size()!=0)
	{
		for(i=0; i< numDofs; i++)	residue[ localGlob[i] ]+= Fq [i];			
	}
}

// ***

void Component::fillResFt(	vector<double>& residue,
					 const double& time_in,
					 const int& modeFlag,
					 const double& grav, const int& gravDir
					 )
{
	int i;
//	if (integratorType==1)	calcFt(time-DeltaT/2.0,grav,gravDir);	// Midpoint
//	else	calcFt(time,grav,gravDir);

	calcFt(time_in,grav,gravDir);
	if(Ft.size()!=0)
	{
		for(i=0; i< numDofs; i++)	residue[ localGlob[i] ]-= Ft [i];
	}	
}

// ***
/*
void Component::fillTangent(sparseMatrix& jac_in, vector<double>& q_in, const double& t, const int& modeFlag)
{
	int i;
	int j;
	
	getLocalDofs(q_in);

	if(modeFlag==1)		// Numerical tangent
	{
		calcKqNumeric();
	} else calcKq(modeFlag);


	if(Kq.size()!=0)
	{
		for(i=0; i< numDofs; i++)
			for(j=0; j< numDofs; j++)					
				jac_in.writeElement( jac_in.readElement(localGlob[i], localGlob[j]) + Kq[i][j], localGlob[i], localGlob[j] );
	}
}
*/
// ***

void Component::fillTangent(sparseMatrix& Kint, vector<vector<double> >& Kbb, vector<vector<double> >& Kib, vector<vector<double> >& Kbi, 
							vector<int>& globalLocalI, vector<int>& globalLocalB,
							vector<double>& q_in, vector<double>& dq_in,
							const double& time, const int& modeFlag)
{
	int i;
	int j;
	
	int a, b;
	
	getLocalDofs(q_in, dq_in);
	
	if(modeFlag==1)		// Numerical tangent
	{
		calcKqNumeric();
	} else calcKq(modeFlag);
	
	if(Kq.size()!=0)
	{
		if (Kbb.size()!=0)
		{
			for(i=0; i< numDofs; i++)
			{
				a = globalLocalI[ localGlob[i] ];	// a!=-1 means interior dof
				for(j=0; j< numDofs; j++)
				{
					b =	globalLocalI[ localGlob[j] ];	// b!=-1 means interior dof
					if (a!=-1 && b!=-1)	// Belongs to Kint
					{
					//cout << "belongs to Kint " << endl;
						Kint.writeElement( Kint.readElement(a,b) + Kq(i,j), a, b );
					}
					else if (a==-1 && b==-1)	// Belongs to Kbb
					{
						//cout << "belongs to Kbb " << endl;
						Kbb[ globalLocalB[ localGlob[i] ] ][ globalLocalB[ localGlob[j] ] ] += Kq(i,j);
					}
					else if (a!=-1 && b==-1)	// Belongs to Kib
					{
						//cout << "belongs to Kib " << endl;
						Kib[ globalLocalI[ localGlob[i] ] ][ globalLocalB[ localGlob[j] ] ] += Kq(i,j);
					}
					else if ( (a==-1 && b!=-1) && Kbi.size()!=0 )	// Belongs to Kbi
					{
						//cout << "belongs to Kbi " << endl;
						Kbi[ globalLocalB[ localGlob[i] ] ][ globalLocalI[ localGlob[j] ] ] += Kq(i,j);
					}
				}
			}
		}
		else	// Just one process, no boundaries
		{
			for(i=0; i< numDofs; i++)
			{
				for(j=0; j< numDofs; j++)
				{
					//cout << "Kq[" << i << "][" << j << "] = " << Kq[i][j] << endl;
					Kint.writeElement( Kint.readElement(localGlob[i], localGlob[j]) + Kq(i,j), localGlob[i], localGlob[j]);
				}
			}
		}
	}
}

// ***

void Component::fillTangent	(sparseMatrix& Kint, vector<vector<double> >& Kbb, vector<vector<double> >& Kib, vector<vector<double> >& Kbi,
							 vector<int>& globalLocalI, vector<int>& globalLocalB,	
							 const vector<double>& q,
							 const vector<double>& qdot,
							 const vector<double>& qddot,
							 double partial_qdot,
							 double partial_qddot,
							 const double& t,
							 const int& modeFlag,
							 const int& iType,
							 const double& DeltaT
							 )
{
	int i;
	int j;	
	double coef;
	int a, b;
	
	if(integratorType==0) coef = 1.0; 
	else coef = DeltaT;
	
	getLocalDofs(q, qdot);
	
	calcKi(partial_qddot);
	calcKdq(partial_qdot);
	
	if(modeFlag==1)		// Numerical tangent
		// WARNING: Only Kq is computed numerically since Ki is constant, and Kdq is empty in all components implemented up to date
	{
		calcKqNumeric();
	} else 
	{
		calcKq(modeFlag);
	}
//
//	cout << "Kq= " << endl << Kq << endl;
//	cout << "numDofs= " << numDofs << endl;
	if(Kq.size()!=0)
	{
		if (Kbb.size()!=0)
		{
			for(i=0; i< numDofs; i++)
			{
				a = globalLocalI[ localGlob[i] ];	// a!=-1 means interior dof
				for(j=0; j< numDofs; j++)
				{
					b =	globalLocalI[ localGlob[j] ];	// b!=-1 means interior dof
					if (a!=-1 && b!=-1)	// Belongs to Kint
					{
						Kint.writeElement( Kint.readElement(a,b) + coef*Kq(i,j), a, b );
					}
					else if (a==-1 && b==-1)	// Belongs to Kbb
					{
						Kbb[ globalLocalB[ localGlob[i] ] ][ globalLocalB[ localGlob[j] ] ] += coef*Kq(i,j);
					}
					else if (a!=-1 && b==-1)	// Belongs to Kib
					{
						Kib[ globalLocalI[ localGlob[i] ] ][ globalLocalB[ localGlob[j] ] ] += coef*Kq(i,j);
					}
					else if ( (a==-1 && b!=-1) && Kbi.size()!=0 )	// Belongs to Kbi
					{
						Kbi[ globalLocalB[ localGlob[i] ] ][ globalLocalI[ localGlob[j] ] ] += coef*Kq(i,j);
					}
				}
			}
		}
		else	// Just one process, no boundaries
		{
			for(i=0; i< numDofs; i++)
			{
				for(j=0; j< numDofs; j++)
				{
					Kint.writeElement( Kint.readElement(localGlob[i], localGlob[j]) + coef*Kq(i,j), localGlob[i], localGlob[j]);
				}
			}
		}		
	}

	if(Ki.size()!=0)
	{
		if (Kbb.size()!=0)
		{
			for(i=0; i< numDofs; i++)
			{
				a = globalLocalI[ localGlob[i] ];	// a!=-1 means interior dof
				for(j=0; j< numDofs; j++)
				{
					b =	globalLocalI[ localGlob[j] ];	// b!=-1 means interior dof
					if (a!=-1 && b!=-1)	// Belongs to Kint
					{
						Kint.writeElement( Kint.readElement(a,b) + coef*Ki(i,j), a, b );
					}
					else if (a==-1 && b==-1)	// Belongs to Kbb
					{
						Kbb[ globalLocalB[ localGlob[i] ] ][ globalLocalB[ localGlob[j] ] ] += coef*Ki(i,j);
					}
					else if (a!=-1 && b==-1)	// Belongs to Kib
					{
						Kib[ globalLocalI[ localGlob[i] ] ][ globalLocalB[ localGlob[j] ] ] += coef*Ki(i,j);
					}
					else if ( (a==-1 && b!=-1) && Kbi.size()!=0 )	// Belongs to Kbi
					{
						Kbi[ globalLocalB[ localGlob[i] ] ][ globalLocalI[ localGlob[j] ] ] += coef*Ki(i,j);
					}
				}
			}
		}
		else	// Just one process, no boundaries
		{
			for(i=0; i< numDofs; i++)
			{
				for(j=0; j< numDofs; j++)
				{
					Kint.writeElement( Kint.readElement(localGlob[i], localGlob[j]) + coef*Ki(i,j), localGlob[i], localGlob[j]);
				}
			}
		}				
	}
	if(Kdq.size()!=0)
	{
		if (Kbb.size()!=0)
		{
			for(i=0; i< numDofs; i++)
			{
				a = globalLocalI[ localGlob[i] ];	// a!=-1 means interior dof
				for(j=0; j< numDofs; j++)
				{
					b =	globalLocalI[ localGlob[j] ];	// b!=-1 means interior dof
					if (a!=-1 && b!=-1)	// Belongs to Kint
					{
						Kint.writeElement( Kint.readElement(a,b) + coef*Kdq(i,j), a, b );
					}
					else if (a==-1 && b==-1)	// Belongs to Kbb
					{
						Kbb[ globalLocalB[ localGlob[i] ] ][ globalLocalB[ localGlob[j] ] ] += coef*Kdq(i,j);
					}
					else if (a!=-1 && b==-1)	// Belongs to Kib
					{
						Kib[ globalLocalI[ localGlob[i] ] ][ globalLocalB[ localGlob[j] ] ] += coef*Kdq(i,j);
					}
					else if ( (a==-1 && b!=-1) && Kbi.size()!=0 )	// Belongs to Kbi
					{
						Kbi[ globalLocalB[ localGlob[i] ] ][ globalLocalI[ localGlob[j] ] ] += coef*Kdq(i,j);
					}
				}
			}
		}
		else	// Just one process, no boundaries
		{
			for(i=0; i< numDofs; i++)
			{
				for(j=0; j< numDofs; j++)
				{
					Kint.writeElement( Kint.readElement(localGlob[i], localGlob[j]) + coef*Kdq(i,j), localGlob[i], localGlob[j]);
				}
			}
		}				
	}
}

// ***

void Component::fillMassMatrix(sparseMatrix& Ma, const vector<double>& q, const double& t)
{
	int i;
	int j;

/*
	cout << "Este elemento tiene numDofsInertia = " << numDofsInertia << endl;
	cout << "localGlob es = " << endl;
	printSTLVector(localGlob);
	
	cout << "localGlobInertia es = " << endl;
	printSTLVector(localGlobInertia);
*/
	
	calcM(q,t);
	if(M.size()!=0)
	{
		for(i=0; i< numDofsInertia; i++)
		{
			for(j=0; j< numDofsInertia; j++)
			{
				Ma.writeElement( Ma.readElement( localGlobInertia[i], localGlobInertia[j] ) + M(i,j), localGlobInertia[i], localGlobInertia[j] ); 
			}
		}
	}
}

// ***

void Component::fillMassMatrix(vector<double>& vMa, const vector<double>& q, const double& t)
{
	int i;
	calcMLumped(q,t);
	if (MLumped.size()!=0)
	{
		for(i=0; i< numDofsInertia; i++)
		{
			vMa[localGlobInertia[i]] += MLumped[i]; 
		}
	}
}

// ***

void Component::fillForce(vector<double>& f,
						const vector<double>& q,
						const vector<double>& qdot,
						const double& t,
						const double& grav, const int& gravDir
						)
{
	int i;
	getLocalDofs(q,qdot);
	
	calcFq(1);

	if (augLagFlag==true && Fq.size()!=0)	for(i=0; i<numDofs; i++)	Fq[i]+= FqLag[i];
	
	calcFt(t,grav,gravDir);
/*
	cout << " q in fillFOrce is " << endl;
	for(i=0; i<q.size(); ++i)	cout << " " << q[i];
	cout << endl;
	
	cout << "Fq in fillForce is " << endl;
	printSTLVector(Fq);
*/	
	if(Fq.size()!=0)
	{
		for(i=0; i< numDofs; i++)	f[ localGlob[i] ]-= Fq [i];
	}
	if(Ft.size()!=0)
	{
		for(i=0; i< numDofs; i++)	f[ localGlob[i] ]+= Ft [i];
	}
}

// ***

void Component::getLocalDofs(const vector<double> &q_in)
{
	int i;
	if(q.size()!=0)
	{
		for(i=0; i<numDofs; i++) q[i] = q_in[ localGlob[i] ];
	}
}

// ***

void Component::getLocalDofs(const vector<double> &q_in, const vector<double> &dq_in)
{
	int i;
	if(q.size()!=0)
	{
		for(i=0; i<numDofs; i++) q[i] = q_in[ localGlob[i] ];
	}
	if(dq.size()!=0)
	{
		for(i=0; i<numDofs; i++)	dq[i] = dq_in[ localGlob[i] ];
	}	
}
// ***
/*
void Component::getLocalDofs(const vector<double> &q_in, const vector<double> &dq_in, const vector<double> &qConv_in, const vector<double> &dqConv_in)
{
	int i;
	if(qC.size()==0 && integratorType!=0)	qC.resize(numDofs);
	if(dqC.size()==0 && integratorType!=0)	dqC.resize(numDofs);
	
	if(q.size()!=0)
	{
		for(i=0; i<numDofs; i++)	q[i] = q_in[ localGlob[i] ];
	}
	if(dq.size()!=0)
	{
		for(i=0; i<numDofs; i++)	dq[i] = dq_in[ localGlob[i] ];
	}
	if(integratorType!=0)
	{
		for(i=0; i<numDofs; i++)	qC[i] = qConv_in[ localGlob[i] ];
	}
	if(integratorType!=0)
	{
		for(i=0; i<numDofs; i++)	dqC[i] = dqConv_in[ localGlob[i] ];
	}	
}
*/
// ***

void Component::setLocalGlob()
{
	int k;
	Point* p;
	localGlob.resize(0);
	localGlobInertia.resize(0);
	
	//cout << "pointnames.size" << pointNames.size() << endl;
	int i;
	for(i=0; i<pointNames.size(); i++)	/** Loop on points */
	{
		p = mPoints[pointNames[i]];
		for(k=0; k< (p->globalTNum.size()); k++)	/** Firstly, translational */
		{
			localGlob.push_back(p->globalTNum[k]);
		}
		for(k=0; k< (p->globalRNum.size()); k++)	/** Secondly, rotational */
		{
			localGlob.push_back(p->globalRNum[k]);
		}
		for(k=0; k< (p->globalINum.size()); k++)	/** Dofs that contribute to inertia */
		{
			localGlobInertia.push_back(p->globalINum[k]);
		}		
	}
	
	//cout << "localGlob.size " << localGlob.size() << endl;
	// Component dofs
	for(k=0; k< globalTNum.size(); k++)
	{
		localGlob.push_back(globalTNum[k]);
	}
	

//	cout << "globalTNum.size is " << globalTNum.size() << endl;
//	cout << "localGlob.size is " << localGlob.size() << endl;
//	cout << "localGlobInertia.size is " << localGlobInertia.size() << endl;
//	cout << "numDofs is " << numDofs << endl;
//	cout << "numDofsInertia is " << numDofsInertia << endl;

	
//	if(localGlob.size()!=numDofs) cout << "*WARNING: Potential inconsistency in dofs at object of type component \n";

//	cout << "localGlob.size() = " << localGlob.size() << "\n";
//	cout << "numDofs = " << numDofs << "\n";

//	for(k=0; k<localGlob.size(); k++) cout << "localGlob[" << k << "]= " << localGlob[k] << "\n";

	
}

// ***

void Component::setSparsePattern(vector< vector<int> >& tempIa)
{
	map<string,Point*,less<string> >::iterator k;
//	Point* p;
	vector<int> vNumbersA;
	vector<int> vNumbersB;
	vector<int> vNumbers;
	int i;
	int j;
	int a;
//	int kk=0;
//	cout << "a coger los " << mPoints.size() << " del componente " << endl;
//	k = mPoints.begin();
//	cout << k->second->name << endl;
	for(k=mPoints.begin(); k!=mPoints.end(); k++)
	{
//		cout << "Voy a coger los numeros del punto " << k->second->name << endl;
		vNumbersA = k->second->getGlobalNumbers();
		vNumbersB.resize(vNumbersA.size()+vNumbers.size());

		merge(vNumbersA.begin(),vNumbersA.end(),vNumbers.begin(),vNumbers.end(),vNumbersB.begin());
		vNumbers = vNumbersB;
		vNumbersB.clear();
//		++kk;
	}
	
	// It is neccessary to include the component dofs (if any)

	if (globalTNum.size()!=0)
	{
		vNumbersA = globalTNum;
		vNumbersB.resize(vNumbersA.size()+vNumbers.size());

		merge(vNumbersA.begin(),vNumbersA.end(),vNumbers.begin(),vNumbers.end(),vNumbersB.begin());
		vNumbers = vNumbersB;
		vNumbersB.clear();
//		cout << "He cogido un component dof " << endl;
	}
	
	for (i=0; i<vNumbers.size(); ++i)	// Loop on global dofs
	{
		a = vNumbers[i];
		for (j=0; j<vNumbers.size(); ++j)
		{
			tempIa[a].push_back( vNumbers[j] );
		}
	}
//	cout << "Aqui el tempIa es " << endl;
//	printSTLMatrix(tempIa);
}

// ***

void Component::setSparsePatternInertia(vector< vector<int> >& tempIa)
{
	
	map<string,Point*,less<string> >::iterator k;
	//	Point* p;
	vector<int> vNumbersA;
	vector<int> vNumbersB;
	vector<int> vNumbers;
	int i;
	int j;
	int a;
	for(k=mPoints.begin(); k!=mPoints.end(); k++)
	{
//		cout << "Voy a coger los numeros de Inercia del punto " << k->second->name << endl;
		vNumbersA = k->second->getGlobalInertiaNumbers();
		vNumbersB.resize(vNumbersA.size()+vNumbers.size());
		
		merge(vNumbersA.begin(),vNumbersA.end(),vNumbers.begin(),vNumbers.end(),vNumbersB.begin());
		vNumbers = vNumbersB;
		vNumbersB.clear();
	}
	
	// WARNING: It is assummed that component dofs do not contribute to inertia
	
	for (i=0; i<vNumbers.size(); ++i)	// Loop on global dofs
	{
		a = vNumbers[i];
		for (j=0; j<vNumbers.size(); ++j)
		{
			tempIa[a].push_back( vNumbers[j] );
		}
	}
//	cout << "Aqui el tempIa es " << endl;
//	printSTLMatrix(tempIa);

}

// ***

void Component::changePointName(string& oldName, string& newName)
{
	// If oldName is not in pointNames, it does nothing
	replace(pointNames.begin(),pointNames.end(),oldName,newName);
}

// ***

void Component::calcKqNumeric()
{
	int i;
	int j;
	
	if(Fq.size()!=0)
	{
		if(Kq.size()==0)
		{
			Kq.setZero(numDofs,numDofs);
		}
		for (j=0;j<numDofs; ++j)
		{
			q[j]+= perturbation;
			calcFq(1);	// 1 means no simultaneous tangent computation (if available)
			
			Kq.col(j) = Fq;
			
			q[j]-= 2.0*perturbation;
			calcFq(1);	// 1 means no simultaneous tangent computation (if available)
			
			Kq.col(j) -= Fq;
			 
			q[j]+= perturbation;
		}
		Kq /= (2.0*perturbation);
		calcFq(1);	// 1 means no simultaneous tangent computation (if available)
	}
   // cout << "Kq= " << endl << Kq << endl;
}

// ***

void Component::update(const int& iType, const vector<double>& qConv_in, const vector<double>& dqConv_in)
{
	
	int i;
	
	integratorType = iType;
	
	if(qC.size()==0)        qC.resize(numDofs);
	if(dqC.size()==0)       dqC.resize(numDofs);
	
	for(i=0; i<numDofs; i++)
	{
		qC[i] = qConv_in[ localGlob[i] ];
		dqC[i] = dqConv_in[ localGlob[i] ];
	}
	
	if (augLagFlag==true)
	{
		PhiC = Phi;
		/*
		cout << "Phi is " << endl;
		printSTLVector(Phi);
	
		cout << "Lagrange multipliers are " << endl;
		printSTLVector(vLagMul);
		*/
	}
	
	updateComp();   // Update internal variables (if any) at components
/*	
	int i;
	
	integratorType = iType;
	
	if(qC.size()==0)	qC.resize(numDofs);
	for(i=0; i<numDofs; i++)	qC[i] = qConv_in[ localGlob[i] ];

	if (dqConv_in.size()!=0)
	{
		if(dqC.size()==0)	dqC.resize(numDofs);
		for(i=0; i<numDofs; i++)	dqC[i] = dqConv_in[ localGlob[i] ];
		getLocalDofs(qConv_in,dqConv_in);	// Dynamics
	}
	else 
	{
		getLocalDofs(qConv_in);	// Statics
	}
	
	updateComp();	// Update internal variables (if any) at components
*/
}

// ***

void Component::setTemperature(double& th)
{
	thetab = th;
}

// ***

int Component::setTRNumbers(int& num)
{
	int i;
	for(i=0; i< dofT.count(); i++) 
	{
		globalTNum.push_back(num+i);
//		cout << "name " << num+i << endl;
	}

//cout << "dofT.count " << dofT.count() << endl;
//cout << "wwwwww = " << num + dofT.count() << endl;

	return( num + dofT.count() );
//	num+=dofT.count();
//	cout << "num al salir de setTR " << num << endl;
}

// ***

void Component::updateDofs(vector<double>& q, vector<double>& deltaq)
{
//   printSTLVector(deltaq);
	int i;
	for (i=0; i<dofT.count(); ++i)
	{
		q[ globalTNum[i] ] -= deltaq[ globalTNum[i] ];
	}	
}	

// ***

int Component::setInertiaNumbers(int num, vector<int>& v)
{
	int i;
	
	// It is assumed that component dofs do not have inertia
	// Its global dof numbers are collected in v
	
	for(i=0; i<globalTNum.size(); ++i)	v.push_back(globalTNum[i]);
	
	return num;
}

// ***

void Component::updateLagMul(void)
{
	int i;
	
	if(integratorType!=1)	calcPhi(q);
	else
	{
	  vector<double> qMid(q.size());
		 for(i=0; i<qMid.size();i++) qMid[i]=0.5*(q[i] + qC[i]);
		 
		 calcPhi(qMid);
	}
	
	cout << "En updateLagMul, Phi es " << endl;
	printSTLVector(Phi);
	
	cout << "En updateLagMul, Lag es ANTES " << endl;
	printSTLVector(vLagMul);

	
	  if(integratorType!=2) 	for (i=0; i<Phi.size(); ++i)	vLagMul[i] += penalty*Phi[i];
	  else	for(i=0; i<Phi.size(); i++)	vLagMul[i] += penalty*(Phi[i] + PhiC[i])/2.0;

	if (augLagFlag==true)
	{
		cout << "q is " << endl;
		printSTLVector(q);
		
		cout << "Phi is " << endl;
		printSTLVector(Phi);
		
		cout << "Lagrange multipliers are " << endl;
		printSTLVector(vLagMul);
	}
	
/*	
	cout << "En updateLagMul, Lag es DESPUES " << endl;
	printSTLVector(vLagMul);
*/
}

// ***

void Component::checkConvLagMul(bool& c)
{
	double tol = 1.0e-6;

	//cout << "FqRatio = " << FqRatio << endl;
	
	if (FqRatio >= tol)	c = false;	// no converged
}

// ***
/*
void Component::projectVel(sparseMatrix& M, vector<double>& dq)
{
	int i;
	int j;
	int k;


//	for (i=0; i<numDofs; ++i)
//	{
//		for(j=0; j<numDofs; ++j)	cout << "M[" << i << "][" << j <<"] = " << M.readElement(localGlobInertia[i], localGlobInertia[j]) << endl;
//	}


//	if (A.size()==0)	// Build projection matrix A
//	{
//		A.resize(numDofsInertia);
//		for (i=0; i<numDofsInertia; ++i)
//		{
//			A[i].resize(numDofsInertia);
//			for (j=0; j<numDofs; ++j)	A[i][j] = M.readElement(localGlobInertia[i],localGlobInertia[j]);
//		}
//	}

	for (i=0; i<numDofsInertia; ++i)
	{
		for (j=0; j<numDofsInertia; ++j)	A[i][j] = M.readElement(localGlobInertia[i],localGlobInertia[j]);
	}
		
//	cout << "Building projection ... " << endl;
	
	// Build P = A + penalty*dPhi*dPhi
	calcDPhi(q);
	vector< vector<double> > P;
	P.resize(numDofsInertia);
	for (i=0; i<numDofsInertia; ++i)	P[i].resize(numDofsInertia);
			
	for (i=0; i<numDofsInertia; ++i)
	{
		for (j=0; j<numDofsInertia; ++j)
		{
			P[i][j] = 0.0;
			for (k=0; k<dPhi.size(); ++k)
			{
				P[i][j]+= penalty*dPhi[k][i]*dPhi[k][j];
			}
			P[i][j]+= A[i][j];
		}
	}
	
//	cout << "Building b... " << endl;
	
	// Build b = A*dq
	vector<double> b;
	b.resize(numDofsInertia);
	for (i=0; i<numDofsInertia; ++i)
	{
		b[i] = 0.0;
		for (j=0; j<numDofsInertia; ++j)
		{
			b[i]+= A[i][j]*dq[localGlobInertia[j]];
		}
	}
	
//	cout << "solving... " << endl;
	
	// Solve projection
	lu(P);
	forSub(P,b);
	backSub(P,b);
	
//	cout << "substitution in dq ... " << endl;
	
//	cout << "dq before... " << endl;
//	printSTLVector(dq);
	
	// Substitution in the velocity vector
	for (i=0; i<numDofsInertia; ++i)
	{
		dq[ localGlobInertia[i] ] = b[i];
	}
//	cout << "dq after.... " << endl;
//	printSTLVector(dq);

}

// ***

void Component::setConstraintNumbers(vector<int>& v)
{
	int i;
	
	for(i=0; i<localGlob.size(); ++i)	v.push_back(localGlob[i]);
	
}
*/

// ***

void Component::calcProjMatrix(sparseMatrix& A, double& projPenal)
{
	int i;
	int j;
	int k;
	double aux;
/*	
	cout << "Component receives projection matrix " << endl;
	for (i=0; i<A.rows(); ++i)
	{
		for (j=0; j<A.rows(); ++j)
		{
			cout << A.readElement(i, j) << " ";
		}
		cout << endl;
	}
	cout << endl;
*/	
	calcDPhi(q);

	if (dPhi.size()!=0)
	{	
//		cout << "dPhi is " << endl;
//		printSTLMatrix(dPhi);
		
		// For just one constraint equation, dPhi is a row matrix
		for (i=0; i<dPhi[0].size(); ++i)
		{
			for (j=0; j<dPhi[0].size(); ++j)
			{
				aux = 0.0;
				for (k=0; k<dPhi.size(); ++k)
				{
					aux+= projPenal*dPhi[k][i]*dPhi[k][j];
					//			double kk = A.readElement(localGlobInertia[i], localGlobInertia[j]);
				}
				if (aux!=0.0)
				{
					A.writeElement(A.readElement(localGlobInertia[i], localGlobInertia[j]) + aux, localGlobInertia[i], localGlobInertia[j]);
				}
			}
		}
	}
}	

// ***

void Component::calcCon(vector<double>& vcon, vector<double>& q_in, vector<double>& dq_in, vector<double>& ddq_in, double& time_in)
{
	int i;
	int j;
	double aux;
	vector<double> auxq(numDofs);
	for (i=0; i<numDofs; ++i)	auxq[i] = q_in[localGlob[i]];
	
	//cout << "en component " << lTwoNorm(dq_in) << endl;
	
	// Constraint (Square of the modulus)
	calcPhi(auxq);
	if (Phi.size()!=0)
	{
		vcon[0] = lTwoNormSq(Phi);
//		cout << "Phi is " << endl;
//		printSTLVector(Phi);
	}
	
	// Velocity constraint	dPhi*dq+dPhit
	// dPhit is the partial derivative of the constraint with time. TO BE DONE
	calcDPhi(auxq);
	if(dPhi.size()!=0)
	{
		// For just one constraint equation, dPhi is a row matrix
		for (i=0; i<dPhi.size(); ++i)
		{
			aux = 0.0;
			for (j=0; j<dPhi[0].size(); ++j)
			{
				aux+= dPhi[i][j]*dq_in[localGlob[j]];
			}
			vcon[1]+= aux*aux;
			//cout << "aux is " << aux << endl;
		}		
	}
	// Acceleration constraint	dot-dPhi*dq + dPhi*ddot-q + ddPhit
	// TO BE DONE

}	

// ***
/*
void Component::calcDeltaEp(sparseMatrix& MfacProj)
{
	int i;
	int j;
	int k;
	double auxd;

	vector<vector<double> > aux = dPhi;
	vector<vector<double> > Gamma;
	
	// Gamma is r x r, being r the number of constraint equations.  r=Phi.size()
	Gamma.resize(Phi.size());
	for (i=0; i<Phi.size(); ++i)	Gamma[i].resize(Phi.size());

	cout << "aux es " << endl;
	printSTLMatrix(aux);

	// M*x = dPhiT
	// Warning: aux is dPhiT (T means transposed)
	MfacProj.subsSolveTrans(aux);
	
	cout << "La solución es " << endl;
	printSTLMatrix(aux);
	
	cout << "Phi.size() is " << Phi.size() << endl;
	// Gamma = dPhi*x
	for (i=0; i<Phi.size(); ++i)
	{
		for(j=0; j<Phi.size(); ++j)
		{
			Gamma[i][j] = 0.0;
			for(k=0; k<numDofs; ++k)
			{
				// Warning: aux is dPhiT (T means transposed)
				Gamma[i][j] += dPhi[i][k]*aux[j][k];
			}
		}
	}
	cout << "Gamma is " << endl;
	printSTLMatrix(Gamma);
}
*/
// ***

void Component::fillPhiq(vector<vector<double> >& Phiq, int& numd)
{
	int i;
	int j;
	vector<double> aux(numd);
/*
	cout << "numd " << numd << endl;
	cout << "numdofsinertia " << numDofsInertia << endl;
	cout << "localglobInertia " << endl;
	printSTLVector(localGlobInertia);
*/	
	calcDPhi(q);
//	cout << "local dphi = " << endl;
//	printSTLMatrix(dPhi);
	
	for (i=0; i<Phi.size(); ++i)
	{
		setZero(aux);
		for (j=0; j<numDofsInertia; ++j)
		{
			aux[localGlobInertia[j]] = dPhi[i][j];
		}
//		cout << " aux = " << endl;
//		printSTLVector(aux);

		Phiq.push_back(aux);
	}

//	cout << "Phiq que estoy llenando es " << endl;
//	printSTLMatrix(Phiq);
}

// ***

void Component::setPointNames(vector<string>& pNames)
{
	pointNames = pNames;
}

// ***

void Component::fillMSmooth(sparseMatrix& Ma, const vector<double>& q, const double& t)
{
	int i;
	int j;
	calcMSmooth(q,t);
	
	if(MSmooth.size() != 0)
	{
		for(i=0; i< numDofsInertia; i++)
		{
			for(j=0; j< numDofsInertia; j++)
			{
				Ma.writeElement( Ma.readElement( localGlobInertia[i], localGlobInertia[j] ) + MSmooth(i,j), localGlobInertia[i], localGlobInertia[j] );
			}
		}
	}

	//printSparseMatrix(Ma);
}

// ***

void Component::fillFSmooth(vector<vector<double> >& Fs, const vector<double>& q_in, const vector<double>& dq_in, const double& time)
{
	int i;
	int j;
	
	calcFSmooth(q_in,dq_in,time);
	if(FSmooth.size()!=0)
	{
		for(i=0; i< 2; i++)
		{
			for(j=0; j< numDofsInertia; j++)	Fs[i][ localGlobInertia[j] ]+= FSmooth(i,j);
		}
	}
	
}

int Component::checkComponents(int state)
{
	int st = checkState();
	// cout << "state in component is:" << state << endl;
	return (state+st);
}




